{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "[Custom_FiltFilt](https://stackoverflow.com/questions/27220671/matlab-filtfilt-function-implementation-in-java/52742099#52742099)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy\n",
    "\n",
    "def custom_filter(b, a, x):\n",
    "    \"\"\" \n",
    "    Filter implemented using state-space representation.\n",
    "\n",
    "    Assume a filter with second order difference equation (assuming a[0]=1):\n",
    "\n",
    "        y[n] = b[0]*x[n] + b[1]*x[n-1] + b[2]*x[n-2] + ...\n",
    "                         - a[1]*y[n-1] - a[2]*y[n-2]\n",
    "\n",
    "    \"\"\"\n",
    "    # State space representation (transposed direct form II)\n",
    "    A = numpy.array([[-a[1], 1], [-a[2], 0]])\n",
    "    B = numpy.array([b[1] - b[0] * a[1], b[2] - b[0] * a[2]])\n",
    "    C = numpy.array([1.0, 0.0])\n",
    "    D = b[0]\n",
    "\n",
    "    # Determine initial state (solve zi = A*zi + B, see scipy.signal.lfilter_zi)\n",
    "    zi = numpy.linalg.solve(numpy.eye(2) - A, B)\n",
    "\n",
    "    # Scale the initial state vector zi by the first input value\n",
    "    z = zi * x[0]\n",
    "\n",
    "    # Apply filter\n",
    "    y = numpy.zeros(numpy.shape(x))\n",
    "    for n in range(len(x)):\n",
    "        # Determine n-th output value (note this simplifies to y[n] = z[0] + b[0]*x[n])\n",
    "        y[n] = numpy.dot(C, z) + D * x[n]\n",
    "        # Determine next state (i.e. z[n+1])\n",
    "        z = numpy.dot(A, z) + B * x[n]\n",
    "    return y\n",
    "\n",
    "def custom_filtfilt(b, a, x):\n",
    "    # Apply 'odd' padding to input signal\n",
    "    padding_length = 3 * max(len(a), len(b))  # the scipy.signal.filtfilt default\n",
    "    x_forward = numpy.concatenate((\n",
    "        [2 * x[0] - xi for xi in x[padding_length:0:-1]],\n",
    "        x,\n",
    "        [2 * x[-1] - xi for xi in x[-2:-padding_length-2:-1]]))\n",
    "\n",
    "    # Filter forward\n",
    "    y_forward = custom_filter(b, a, x_forward)\n",
    "\n",
    "    # Filter backward\n",
    "    x_backward = y_forward[::-1]  # reverse\n",
    "    y_backward = custom_filter(b, a, x_backward)\n",
    "\n",
    "    # Remove padding and reverse\n",
    "    return y_backward[-padding_length-1:padding_length-1:-1]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy\n",
    "import numpy.testing\n",
    "import scipy.signal\n",
    "from matplotlib import pyplot as plt\n",
    "from matplotlib.pyplot import rc\n",
    "plt.rcParams[\"font.size\"] = 8\n",
    "plt.rcParams['font.sans-serif'] = ['Microsoft YaHei']  # 显示中文\n",
    "plt.rcParams['axes.unicode_minus'] = False\n",
    "#from . import custom_filtfilt\n",
    "\n",
    "def sinusoid(sampling_frequency_Hz=50.0, signal_frequency_Hz=1.0, periods=1.0,\n",
    "             amplitude=1.0, offset=0.0, phase_deg=0.0, noise_std=0.1):\n",
    "    \"\"\"\n",
    "    Create a noisy test signal sampled from a sinusoid (time series)\n",
    "\n",
    "    \"\"\"\n",
    "    signal_frequency_rad_per_s = signal_frequency_Hz * 2 * numpy.pi\n",
    "    phase_rad = numpy.radians(phase_deg)\n",
    "    duration_s = periods / signal_frequency_Hz\n",
    "    number_of_samples = int(duration_s * sampling_frequency_Hz)\n",
    "    time_s = (numpy.array(range(number_of_samples), float) /\n",
    "              sampling_frequency_Hz)\n",
    "    angle_rad = signal_frequency_rad_per_s * time_s\n",
    "    signal = offset + amplitude * numpy.sin(angle_rad - phase_rad)\n",
    "    noise = numpy.random.normal(loc=0.0, scale=noise_std, size=signal.shape)\n",
    "    return signal + noise\n",
    "\n",
    "\n",
    "#if __name__ == '__main__':\n",
    "def my_main():\n",
    "    # Design filter\n",
    "    sampling_freq_hz = 50.0\n",
    "    cutoff_freq_hz = 2.5\n",
    "    order = 2\n",
    "    normalized_frequency = cutoff_freq_hz * 2 / sampling_freq_hz\n",
    "    b, a = scipy.signal.butter(order, normalized_frequency, btype='lowpass')\n",
    "\n",
    "    # Create test signal\n",
    "    signal = sinusoid(sampling_frequency_Hz=sampling_freq_hz,\n",
    "                      signal_frequency_Hz=1.5, periods=3, amplitude=2.0,\n",
    "                      offset=2.0, phase_deg=25)\n",
    "\n",
    "    # Apply zero-phase filters\n",
    "    filtered_custom = custom_filtfilt(b, a, signal)\n",
    "    filtered_scipy = scipy.signal.filtfilt(b, a, signal)\n",
    "\n",
    "    # Verify near-equality\n",
    "    numpy.testing.assert_array_almost_equal(filtered_custom, filtered_scipy,\n",
    "                                            decimal=12)\n",
    "\n",
    "    # Plot result\n",
    "    plt.figure(1, dpi=800)\n",
    "    plt.subplot(1, 2, 1)\n",
    "    plt.plot(signal, linewidth=0.5)\n",
    "    plt.plot(filtered_scipy, linewidth=0.5)\n",
    "    plt.plot(filtered_custom, '.', linewidth=0.5)\n",
    "    plt.title('raw vs filtered signals')\n",
    "    plt.legend(['raw', 'scipy filtfilt', 'custom filtfilt'],loc='upper right')\n",
    "    plt.subplot(1, 2, 2)\n",
    "    plt.plot(filtered_scipy-filtered_custom, linewidth=0.5)\n",
    "    plt.title('difference (scipy vs custom)')\n",
    "    plt.show()\n",
    "    \n",
    "my_main()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
